#include <iterator>

#include "util/outputmacros.h"
#include "DataIO/include/DataReader.h"
#include "util/mathfunc.h"
#include "DataStructure/Frame.h"
#include "DataStructure/LandMarkPLX.h"
#include "DataManager.h"

#include "Optimizer.h"

#include "util/TIMER.h"
#include "IdProvider.h"

using namespace Estimator;
using namespace Eigen;

static IdProvider::instance pose_id;
static IdProvider::instance Landmark_id; 

int main(int argc, char** argv)
{
    Estimator::DataReader datareader(argv[1]);

    // iteration for each frame:
    std::vector<Estimator::DataReader::PoseType> poseContainer = datareader.GetPoseContainer();
    
    std::cout << "=========== Generate data associations from TXT files  ============" << std::endl;
    std::vector<Frame> vFrames;
    for(size_t pose_id = 0; pose_id < poseContainer.size(); pose_id++)
    {

        Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Rot;
        Rot << poseContainer[pose_id][0], poseContainer[pose_id][1], poseContainer[pose_id][2],
               poseContainer[pose_id][3], poseContainer[pose_id][4], poseContainer[pose_id][5],
               poseContainer[pose_id][6], poseContainer[pose_id][7], poseContainer[pose_id][8];

        Eigen::Vector3d vec = Eigen::Vector3d(poseContainer[pose_id][9], poseContainer[pose_id][10], poseContainer[pose_id][11]);

        // Frame frame = Frame(pose_id, )
        auto obsRange = datareader.FindObservationRangeByFrameID(pose_id);
        Frame frame = Frame(pose_id, Rot, vec);

        
        for(auto ite = obsRange.first; ite != obsRange.second; ++ite)
        {
            frame.mvLandMarkObservations_.emplace_back((*ite).first.second, (*ite).second);        
        }

        vFrames.push_back(frame);
    }
    
    // std::cout << vFrames.size() << std::endl;

     // std::cout << 

    std::cout << "========== Generation completed ================" << std::endl;


    Calibration calibration;
    calibration.Rbc = Eigen::Matrix3d::Identity();
    calibration.pbc = Eigen::Vector3d::Zero();
    calibration.K = Eigen::Matrix3d::Zero();
    // Liang's data: 
    calibration.K(0,0) = 923.5295;
    calibration.K(1,1) = 922.2418;
    calibration.K(2,2) = 1;
    calibration.K(0,2) = 507.2222;
    calibration.K(1,2) = 383.5822;


    // Teng's data: 
    // calibration.K(0,0) = 525;       
    // calibration.K(1,1) = 525;
    // calibration.K(2,2) = 1;
    // calibration.K(0,2) = 639.5;
    // calibration.K(1,2) = 479.5;
    calibration.Kinv = calibration.K.inverse();

    Managing Manager(&calibration);

    std::cout << "=========== Add Frames to the Manager ==============" << std::endl;

    for(size_t i = 0; i < vFrames.size(); ++i)
    {
        Manager.AddFrame(&vFrames[i]);
    }

    Manager.RemoveLandMarkWithSingleAnchor();

    std::cout << "Pose id: " << pose_id.currentId() << std::endl;
    std::cout << "LandMark id: " << Landmark_id.currentId() << std::endl;
    std::cout << "Next Pose id: " << pose_id.newId() << std::endl;
    std::cout << "Next LandMark id: " << Landmark_id.newId() << std::endl;

    std::cout << "Manger's landMark : " << Manager.mlLandMarkContainer_.size() << std::endl;
    std::cout << "Manger's Poses : " <<  Manager.mvpFrameContainer_.size() << std::endl;

    std::cout << "============== begin optimization =================" << std::endl;

    Optimizer optimizer(calibration);

    optimizer.InitializeByManager(&Manager);

    std::cout << "============== Generationg Costfunctions =================" << std::endl;
    optimizer.GenerateCostFunctionBeta(&Manager);

    //Manager.WriteLandmarkParallax(optimizer, "before");

    // std::cout << optimizer.nEdge_++ << std::endl;

    std::cout << "run the solver..." << std::endl;

    TIMER solving_time("solving time");
    solving_time.start_timer();
    optimizer.SolveOptimization();
    solving_time.end_timer();
    std::cout << solving_time << std::endl;

    std::vector<std::array<double,12> > poses = optimizer.WritePoses();

    std::fstream fstr("poses.txt", fstr.trunc | fstr.out);
    for(const std::array<double,12>& pose : poses)
    {
        std::copy(pose.cbegin(), pose.cend(), std::ostream_iterator<double>(fstr, " "));
        std::cout << std::endl;
    }
    fstr.close();
    //optimizer.WriteResultintoFile("../GroundTruth");

    //Manager.WriteLandmarkParallax(optimizer, "after");

    std::cout << "==== end ====" << std::endl;
    return 0;
}